//#ifdef __cplusplus
//extern "C" {
//#endif

#include <kfc.h>
// Sensors count
int SNS_CNT = 4;

// State Vector length
int STT_LEN = 8;

MTX A; // init in initialize_kf
/*{
    { 0.9998, 0.0009, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { -0.4434, 0.7537, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.9998, 0.0009, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, -0.4434, 0.7537, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.9998, 0.0009, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, -0.4434, 0.7537, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.9998, 0.0009},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, -0.4434, 0.7537},
     };*/
VECT X; // init in initialize_kf
MTX P; // init in initialize_kf
MTX B;
/*{
    { 0.0002, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.4434, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.0002, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.4434, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.0002, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.4434, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.0002},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.4434},
     };*/
MTX C;
/*{
    { 1.0000, 0.0072, -0.0384, -0.1300},
    { 0.0072, 0.3392, -0.0097, 0.0006},
    { -0.0384, -0.0097, 0.0577, 0.0002},
    { -0.1300, 0.0006, 0.0002, 0.4074},
     };*/
MTX D;
/*{
    { 0.0003, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.0003, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.0001, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.0001, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 1.73e-005, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 1.73e-005, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.0001, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.0001},
     };*/
MTX H;
/*{
    { 1.0000, 0.5500, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 1.0000, 0.5500, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 1.0000, 0.5500, 0.00e+000, 0.00e+000},
    { 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 0.00e+000, 1.0000, 0.5500},
     };*/
MTX K; // init in initialize_kf
MTX N;
/*{
    { 0.6948, -0.6948, 0.1857},
    { 0.4391, 0.4391, 0.7838},
    { -0.6948, 0.6948, 0.1857},
    { -0.3557, -0.3557, 0.8642},
     };*/
MTX NInv; // init in initialize_kf
MTX BN; // init in initialize_kf

VECT U;
/*{0, 0, 0};*/

// Temporaries
VECT Xpred; // init in initialize_kf
MTX Ppred; // init in initialize_kf
MTX k1; // init in initialize_kf
MTX k2; // init in initialize_kf
VECT zHXpred; // init in initialize_kf
VECT k3; // init in initialize_kf
MTX k4; // init in initialize_kf
MTX k5; // init in initialize_kf

extern void initialize_kf(char *pFilePath)
{
    // 1. Read settings and params from config files
    // TODO: Temporarily used initialization with test values
    // Test parameters taken from IronRuby implementation of Kalman Filter (see sINS)
    A = alloc_MTX(STT_LEN, STT_LEN);
    A[0][0] = 0.9998; A[0][1] = 0.0009;
    A[1][0] = -0.4434; A[1][1] = 0.7537;
    A[2][2] = 0.9998; A[2][3] = 0.0009;
    A[3][2] = -0.4434; A[3][3] = 0.7537;
    A[4][4] = 0.9998; A[4][5] = 0.0009;
    A[5][4] = -0.4434; A[5][5] = 0.7537;
    A[6][6] = 0.9998; A[6][7] = 0.0009;
    A[7][6] = -0.4434; A[7][7] = 0.7537;

    X = alloc_VECT(STT_LEN);

    B = alloc_MTX(STT_LEN, SNS_CNT);
    B[0][0] = 0.0002;
    B[1][0] = 0.4434;
    B[2][1] = 0.0002;
    B[3][1] = 0.4434;
    B[4][2] = 0.0002;
    B[5][2] = 0.4434;
    B[6][3] = 0.0002;
    B[7][3] = 0.4434;

    C = alloc_MTX(SNS_CNT, SNS_CNT);
    C[0][0] = 1.0000;   C[0][1] = 0.0072;   C[0][2] = -0.0384;  C[0][3] = -0.1300;
    C[1][0] = 0.0072;   C[1][1] = 0.3392;   C[1][2] = -0.0097;  C[1][3] = 0.0006;
    C[2][0] = -0.0384;  C[2][1] = -0.0097;  C[2][2] = 0.0577;   C[2][3] = 0.0002;
    C[3][0] = -0.1300;  C[3][1] = 0.0006;   C[3][2] = 0.0002;   C[3][3] = 0.4074;

    D = alloc_MTX(STT_LEN, STT_LEN);
    D[0][0] = 0.0003;
    D[1][1] = 0.0003;
    D[2][2] = 0.0001;
    D[3][3] = 0.0001;
    D[4][4] = 1.73e-005;
    D[5][5] = 1.73e-005;
    D[6][6] = 0.0001;
    D[7][7] = 0.0001;

    H = alloc_MTX(SNS_CNT, STT_LEN);
    H[0][0] = 1.0; H[0][1] = 0.55;
    H[1][2] = 1.0; H[1][3] = 0.55;
    H[2][4] = 1.0; H[2][5] = 0.55;
    H[3][6] = 1.0; H[3][7] = 0.55;

    N = alloc_MTX(SNS_CNT, 3);
    N[0][0] = 0.6948; N[0][1] = -0.6948; N[0][2] = 0.1857;
    N[1][0] = 0.4391; N[1][1] = 0.4391; N[1][2] = 0.7838;
    N[2][0] = -0.6948; N[2][1] = 0.6948; N[2][2] = 0.1857;
    N[3][0] = -0.3557; N[3][1] = -0.3557; N[3][2] = 0.8642;

    K = alloc_MTX(STT_LEN, SNS_CNT);
    P = alloc_MTX(STT_LEN, STT_LEN);
    BN = alloc_MTX(STT_LEN, 3);
    mm_mult(STT_LEN, SNS_CNT, B, 3, N, BN);

    U = alloc_VECT(3);
    /*NInv = {
{ -0.0462, 0.7379, -0.7658, -0.4947},
{ -0.1217, 0.6086, 0.5979, -0.6543},
{ 0.1771, 0.5048, 0.1771, 0.6231},
 } */
    // ^^^ Must be checked/tested

    MTX NtN = alloc_MTX(3,3);
    mm_mult_a_transp(SNS_CNT, 3, N, 3, N, NtN); // Written before Nt introduction
    // This call can be replaced with non trapnsposed version of multiplication

    MTX NtN_Nt = alloc_MTX(3, SNS_CNT);
    lu_solve_3_n_b_transp(NtN, SNS_CNT, N, NtN_Nt);

    NInv = alloc_MTX(3, STT_LEN);
    mm_mult(3, SNS_CNT, NtN_Nt, STT_LEN, H, NInv);

    // 2. Initializing matrices and vectors data
    Xpred = alloc_VECT(STT_LEN);
    Ppred = alloc_MTX(STT_LEN, STT_LEN);
    k1 = alloc_MTX(STT_LEN, SNS_CNT);
    k2 = alloc_MTX(SNS_CNT, SNS_CNT);
    zHXpred = alloc_VECT(STT_LEN);
    k3 = alloc_VECT(SNS_CNT);
    k4 = alloc_MTX(SNS_CNT, STT_LEN);
    k5 = alloc_MTX(STT_LEN, STT_LEN);

    // 2. Initialize filter model

    // 3. Cleanup dynamic allocs
    dealloc_MTX(3, NtN);
    dealloc_MTX(3, NtN_Nt);
}

extern void evaluate_step(VECT pZ, VECT pU)
{
    VECT bu = alloc_VECT(STT_LEN);

    // A * X[k-1]
    mv_mult(STT_LEN, STT_LEN, A, X, Xpred);

    // TODO: expression wrong
    // B * U[k-1]
    mv_mult(STT_LEN, 3, B, U, bu); // Right expression is "B*N*U"
    // ^^^ FIX It UP

    // Xpred = A*X[k-1] + B*U[k-1]
    vv_sum_accum(STT_LEN, Xpred, bu);
    free(bu);

    // Ppred = A * P * At + D
    calc_ABAtC(STT_LEN, STT_LEN, A, STT_LEN, P, C, Ppred);

    // Original algorithm
    // K = Ppred * Ht * (H * Ppred * Ht + C)^(-1)
    // Xev = Xpred + K * (z - H * Xpred)
    // P = (I - K * H) * Ppred

    // Decomposed alternative:
    // K1 = Ppred * Ht
    mm_mult_b_transp(STT_LEN, STT_LEN, Ppred, SNS_CNT, H, k1);

    // K2 = H * Ppred * Ht + C
    calc_ABAtC(SNS_CNT, STT_LEN, H, STT_LEN, Ppred, C, k2);

    // K3 = K2^(-1) * (z - H * Xpred)
    mv_mult(SNS_CNT,STT_LEN, H, Xpred, zHXpred);
    int i, j;
    for (i = 0; i < SNS_CNT; ++i) {
        zHXpred[i] = pZ[i] - zHXpred[i];
    }

    // K4 = K2^(-1) * H
#if STT_LEN == 8
    // Hard Coded calculation of LU-Inverse for case when there is 4 sensors with second-order models
    lu_solve_4_1(k2, zHXpred, k3);
    lu_solve_4_n(k2, STT_LEN, H, k4);
#elif STT_LEN == 12
    // Hard Coded calculation of LU-Inverse for case when there is 4 sensors with third-order models
#endif

    // Xev = XPred + K1 * K3
    mv_mult(STT_LEN, SNS_CNT, k1, k3, X);
    for (i = 0; i < SNS_CNT; ++i) {
        X[i] = X[i] + Xpred[i];
    }

    // P = (I - K1 * K4) * Ppred
    mm_mult(STT_LEN, SNS_CNT, k1, STT_LEN, k4, k5);
    for (i = 0; i < STT_LEN; ++i) {
        for (j = 0; j < STT_LEN; ++j) {
            // I - K1 * K4
            int add_val = i == j ? 1 : 0;
            k5[i][j] = add_val - k5[i][j];
        }
    }
    mm_mult(STT_LEN, STT_LEN, k5, STT_LEN, Ppred, P);

    // TODO: State Vector to Input Vector inverse problem solution (using Gauss-Markov pseudo-inverse)
    // U = (Nt * N)^-1 * Nt * H * X
    mv_mult(3, STT_LEN, NInv, X, U);

    // Copying Input Vector evaluation to external buffer pU
    for (i = 0; i < 3; ++i) {
        pU[i] = U[i];
    }
}

extern void release_kf()
{
    dealloc_MTX(STT_LEN, A);
    free(X);
    dealloc_MTX(STT_LEN, P);
    dealloc_MTX(STT_LEN, B);
    dealloc_MTX(SNS_CNT, C);
    dealloc_MTX(STT_LEN, D);
    dealloc_MTX(SNS_CNT, H);
    dealloc_MTX(STT_LEN, K);
    dealloc_MTX(SNS_CNT, N);
    dealloc_MTX(3, NInv);

    // Temporaries Cleanup
    dealloc_MTX(STT_LEN, Ppred);
    dealloc_MTX(STT_LEN, k1);
    dealloc_MTX(SNS_CNT, k2);
    dealloc_MTX(SNS_CNT, k4);
    dealloc_MTX(STT_LEN, k5);

    free(Xpred);
    free(zHXpred);
    free(k3);


}

extern void calc_ABAtC(int rows_a, int cols_a, MTX A, int cols_b, MTX B, MTX C, MTX res)
{
    // A * B * At
    MTX temp1 = alloc_MTX(rows_a, cols_b);
    mm_mult(rows_a, cols_a, A, cols_b, B, temp1);

    // res = A * B * At
    mm_mult_b_transp(rows_a, cols_b, temp1, rows_a, A, res);

    // res += C
    // temp var 'temp1' reused to store result of addition
    mm_add_sub(rows_a, rows_a, res, C, res, 1);

    dealloc_MTX(rows_a, temp1);
}

//#ifdef __cplusplus
//}
//#endif
